/*
 * Copyright (C) 2008, Morgan Quigley and Willow Garage, Inc.
 *
 * Redistribution and use in source and binary forms, with or without
 * modification, are permitted provided that the following conditions are met:
 *   * Redistributions of source code must retain the above copyright notice,
 *     this list of conditions and the following disclaimer.
 *   * Redistributions in binary form must reproduce the above copyright
 *     notice, this list of conditions and the following disclaimer in the
 *     documentation and/or other materials provided with the distribution.
 *   * Neither the names of Stanford University or Willow Garage, Inc. nor the names of its
 *     contributors may be used to endorse or promote products derived from
 *     this software without specific prior written permission.
 *
 * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
 * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
 * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
 * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE
 * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
 * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
 * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
 * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
 * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
 * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
 * POSSIBILITY OF SUCH DAMAGE.
 */
// %Tag(FULLTEXT)%
// %Tag(ROS_HEADER)%
#include "ros/ros.h"
// %EndTag(ROS_HEADER)%
// %Tag(MSG_HEADER)%
#include "std_msgs/String.h"
#include "std_msgs/Int32MultiArray.h"
// %EndTag(MSG_HEADER)%

#include <sstream>
#include <time.h>
#include <stdio.h>
#include <iostream>
#include <string.h>

#include <sys/types.h>
#include <sys/stat.h>
#include <fcntl.h>
#include <termios.h>


#include <stdlib.h>
#include <math.h>
#include <unistd.h>
#include <fcntl.h>
#include <errno.h>
#include <pthread.h>
#include <cstdlib>

#define BAUDRATE B57600 //
#define IMU_PORT "/dev/ttyUSB0"
#define FALSE 0
#define TRUE 1

using namespace std;
FILE* fid;

volatile int STOP=FALSE;
struct termios oldtio,newtio;
time_t t_start, t_end, t1_start;
string str("");
int pbuf[31];
int pre_pbuf[9]={0,0,0,0,0,0,0,0,0};
int bias_pbuf[31]={0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0};
int seqs=0;
int time_cnt=0;

std_msgs::Int32MultiArray msg;
std_msgs::Int32MultiArray msg_imu;

void logging()
{
	char dir[100]="";
	time_t rawtime;
	struct tm * timeinfo;

	time ( &rawtime );
	timeinfo = localtime ( &rawtime );
	if (timeinfo->tm_sec<55)
	{
	sprintf ( dir ,"/home/pilot/log/%4d_%02d_%02d_%02d%02d",  timeinfo->tm_year+1900, timeinfo->tm_mon+1, timeinfo->tm_mday, timeinfo->tm_hour, timeinfo->tm_min);
	}
	else
	{
		sprintf ( dir ,"/home/pilot/log/%4d_%02d_%02d_%02d%02d",  timeinfo->tm_year+1900, timeinfo->tm_mon+1, timeinfo->tm_mday, timeinfo->tm_hour, timeinfo->tm_min+1);

	}
	mkdir(dir,0777);//creating a directory

	char log[100]="";
	char format[30]="/imu_debug_data.txt";
	strcat(log,dir);
	strcat(log,format);
	
	if((fid=fopen(log,"w+"))==NULL)
		 cout<<"Fail to open the file!"<<endl;

}

int decode(string str1){
	
	char rxd_buffer[100];
	unsigned char buffer[100];
	str1.copy(rxd_buffer,str1.length());
	//rxd_buffer[83]=0;
	//buffer[83]=0;
//	cout<<"\nReceived string: "<<rxd_buffer<<endl;
	unsigned char a,b,c,d;
	int ptrIn = 3;
	int ptrOut = 3;
	int len = str1.length()+1-6;
	buffer[0]='#';
	buffer[1]='#';
	buffer[2]='#';
	while (len)
	{
		a = rxd_buffer[ptrIn++] - '=';
		b = rxd_buffer[ptrIn++] - '=';
		c = rxd_buffer[ptrIn++] - '=';
		d = rxd_buffer[ptrIn++] - '=';
		//if(ptrIn > ReceivedBytes - 3) break;
/*
		unsigned char a1 = a;
		unsigned char b1 = b;
		unsigned char c1 = c;
		unsigned char d1 = d;
		printf("%d %d %d %d ",a1,b1,c1,d1);
*/
		unsigned char x = (a << 2) | (b >> 4);
		unsigned char y = ((b & 0x0f) << 4) | (c >> 2);
		unsigned char z = ((c & 0x03) << 6) | d;
		if (len--){
			buffer[ptrOut++] = x;
		//	printf("%d ",x);
		}
		else
			break;
		if (len--){
			buffer[ptrOut++] = y; 
		//	printf("%d ",y);
		}
		else
			break;
		if (len--){
			buffer[ptrOut++] = z;
		//	printf("%d ",z);
		}
		else
			break;
	}
/*
	int angleNick;
	if (buffer[6]>128)
		angleNick = buffer[5]-256-(255-buffer[6])*256;	
	else
		angleNick = buffer[5]+buffer[6]*256;	


	int angleRoll;
	if (buffer[8]>128)
		angleRoll = buffer[7]-256-(255-buffer[8])*256;	
	else
		angleRoll = buffer[7]+buffer[8]*256;	

	
	int accNick;
	if (buffer[10]>128)
		accNick = buffer[9]-256-(255-buffer[10])*256;	
	else
		accNick = buffer[9]+buffer[10]*256;	
	
	
	int accRoll;
	if (buffer[12]>128)
		accRoll = buffer[11]-256-(255-buffer[12])*256;	
	else
		accRoll = buffer[11]+buffer[12]*256;	


	int yawGyro;
	if (buffer[14]>128)
		yawGyro = buffer[13]-256-(255-buffer[14])*256;	
	else
		yawGyro = buffer[13]+buffer[14]*256;	
	
	
	int height;
	if (buffer[16]>128)
		height = buffer[15]-256-(255-buffer[16])*256;	
	else
		height = buffer[15]+buffer[16]*256;	


	int accZ;
	if (buffer[18]>128)
		accZ = buffer[17]-256-(255-buffer[18])*256;	
	else
		accZ = buffer[17]+buffer[18]*256;	
	

	int Gas;
	if (buffer[20]>128)
		Gas = buffer[19]-256-(255-buffer[20])*256;	
	else
		Gas = buffer[19]+buffer[20]*256;	
	

	int compass;
	if (buffer[22]>128)
		compass = buffer[21]-256-(255-buffer[22])*256;	
	else
		compass = buffer[21]+buffer[22]*256;	

	int volt;
	if (buffer[24]>128)
		volt = buffer[23]-256-(255-buffer[24])*256;	
	else
		volt = buffer[23]+buffer[24]*256;	


	int Receiver;
	if (buffer[26]>128)
		Receiver = buffer[25]-256-(255-buffer[26])*256;	
	else
		Receiver = buffer[25]+buffer[26]*256;	


	int gyro_compass;
	if (buffer[28]>128)
		gyro_compass = buffer[27]-256-(255-buffer[28])*256;	
	else
		gyro_compass = buffer[27]+buffer[28]*256;	
	
	int Motor1;
	if (buffer[30]>128)
		Motor1 = buffer[29]-256-(255-buffer[30])*256;	
	else
		Motor1 = buffer[29]+buffer[30]*256;	
	
	int Motor2;
	if (buffer[32]>128)
		Motor2 = buffer[31]-256-(255-buffer[32])*256;	
	else
		Motor2 = buffer[31]+buffer[32]*256;	
	
	int Motor3;
	if (buffer[34]>128)
		Motor3 = buffer[33]-256-(255-buffer[34])*256;	
	else
		Motor3 = buffer[33]+buffer[34]*256;	
	

	int Motor4;
	if (buffer[36]>128)
		Motor4 = buffer[35]-256-(255-buffer[36])*256;	
	else
		Motor4 = buffer[35]+buffer[36]*256;	
//	printf("Motor4: %d\n",Motor4);
*/
//	fseek(fid,0,SEEK_END);
//	fprintf(fid,"%f",ros::Time::now().toSec());	
	
	int mm=0;
	int tmp_buf=0;
	len = str1.length()+1-6;
	for (int i=0;mm<=15;){
		 
		if (buffer[5+i+1]>128)
			pbuf[mm++] = buffer[5+i]-256-(255-buffer[5+i+1])*256;	
		else
			pbuf[mm++] = buffer[5+i]+buffer[5+i+1]*256;
		i=i+2;
		if ((seqs==0)&&(mm-1<9))
		{
			bias_pbuf[mm-1]=pbuf[mm-1];
			seqs++;
		//	bias_pbuf[mm-1]=bias_pbuf[mm-1]/2;	
			
		}


		if ((mm==3)||(mm==4))
		{
			//pbuf[mm-1]=pbuf[mm-1]-bias_pbuf[mm-1];
			pbuf[mm-1]=pbuf[mm-1]-pbuf[mm-3]-bias_pbuf[mm-1];
//			fseek(fid,0,SEEK_END);
//			fprintf(fid,",%d",pbuf[mm-1]);		
			//printf("%d\n",pbuf[mm-1]);

		}
		else
		{
			pbuf[mm-1]=pbuf[mm-1]-bias_pbuf[mm-1];
//			fseek(fid,0,SEEK_END);
//			fprintf(fid,",%d",pbuf[mm-1]);
 			//printf("%d\n",pbuf[mm-1]);
		}
				

			

		switch(mm-1)
		{
			case 0:	msg.data.push_back(pbuf[mm-1]); msg_imu.data.push_back(pbuf[mm-1]); break;
			case 1: msg.data.push_back(pbuf[mm-1]);	msg_imu.data.push_back(pbuf[mm-1]); break;
//			case 2: msg.data.push_back(tmp_buf);	break;
//			case 3: msg.data.push_back(tmp_buf);	break;
			case 11: msg.data.push_back(pbuf[mm-1]); msg_imu.data.push_back(pbuf[mm-1]); break;
			default: msg.data.push_back(pbuf[mm-1]);
		}
	}


//	cout<<"end\n";

//	fseek(fid,0,SEEK_END);
//	fprintf(fid,"%s\n",";");
	return 1;
	
}

/**
 * This tutorial demonstrates simple sending of messages over the ROS system.
 */
int main(int argc, char **argv)
{
//	logging();
	int start;
//	//fseek(fid,0,SEEK_END);
//	//fprintf(fid,"%s;\n","TimeStamp, AnglePitch, AngleRoll, GyroPitch, GyroRoll, GyroYaw, Height, AccZ, Thrust, Compass, Volt, Receiver, Gyrocompass, Motor1-4");

	int fd=open(IMU_PORT, O_RDWR | O_NOCTTY);
	if (fd<0){
		perror(IMU_PORT);
		return -1;
	}
	tcgetattr(fd,&oldtio); /* save current serial port settings */
        //bzero(&newtio, sizeof(newtio)); /* clear struct for new port settings */

	/* 
          BAUDRATE: Set bps rate. You could also use cfsetispeed and cfsetospeed.
          CRTSCTS : output hardware flow control (only used if the cable has
                    all necessary lines. See sect. 7 of Serial-HOWTO)
          CS8     : 8n1 (8bit,no parity,1 stopbit)
          CLOCAL  : local connection, no modem contol
          CREAD   : enable receiving characters
        */
         newtio.c_cflag = BAUDRATE | CRTSCTS | CS8 | CLOCAL | CREAD;

	/*
          IGNPAR  : ignore bytes with parity errors
          ICRNL   : map CR to NL (otherwise a CR input on the other computer
                    will not terminate input)
          otherwise make device raw (no other input processing)
        */
         newtio.c_iflag = IGNPAR | ICRNL;

	/*
         Raw output.
        */
         newtio.c_oflag = 0;
         
        /*
          ICANON  : enable canonical input
          disable all echo functionality, and don't send signals to calling program
        */
         newtio.c_lflag = ICANON;
	
	/* 
          initialize all control characters 
          default values can be found in /usr/include/termios.h, and are given
          in the comments, but we don't need them here
        */
         newtio.c_cc[VINTR]    = 0;     /* Ctrl-c */ 
         newtio.c_cc[VQUIT]    = 0;     /* Ctrl-\ */
         newtio.c_cc[VERASE]   = 0;     /* del */
         newtio.c_cc[VKILL]    = 0;     /* @ */
         newtio.c_cc[VEOF]     = 4;     /* Ctrl-d */
         newtio.c_cc[VTIME]    = 0;     /* inter-character timer unused */
         newtio.c_cc[VMIN]     = 1;     /* blocking read until 1 character arrives */
         newtio.c_cc[VSWTC]    = 0;     /* '\0' */
         newtio.c_cc[VSTART]   = 0;     /* Ctrl-q */ 
         newtio.c_cc[VSTOP]    = 0;     /* Ctrl-s */
         newtio.c_cc[VSUSP]    = 0;     /* Ctrl-z */
         newtio.c_cc[VEOL]     = 0;     /* '\0' */
         newtio.c_cc[VREPRINT] = 0;     /* Ctrl-r */
         newtio.c_cc[VDISCARD] = 0;     /* Ctrl-u */
         newtio.c_cc[VWERASE]  = 0;     /* Ctrl-w */
         newtio.c_cc[VLNEXT]   = 0;     /* Ctrl-v */
         newtio.c_cc[VEOL2]    = 0;     /* '\0' */
        
        /* 
          now clean the modem line and activate the settings for the port
        */
         tcflush(fd, TCIFLUSH);
         tcsetattr(fd,TCSANOW,&newtio);
	 fcntl(fd, F_SETFL, FNDELAY);



// %Tag(INIT)%
  ros::init(argc, argv, "imu_debug");
// %EndTag(INIT)%

  /**
   * NodeHandle is the main access point to communications with the ROS system.
   * The first NodeHandle constructed will fully initialize this node, and the last
   * NodeHandle destructed will close down the node.
   */
// %Tag(NODEHANDLE)%
  ros::NodeHandle imu;
// %EndTag(NODEHANDLE)%

  /**
   * The advertise() function is how you tell ROS that you want to
   * publish on a given topic name. This invokes a call to the ROS
   * master node, which keeps a registry of who is publishing and who
   * is subscribing. After this advertise() call is made, the master
   * node will notify anyone who is trying to subscribe to this topic name,
   * and they will in turn negotiate a peer-to-peer connection with this
   * node.  advertise() returns a Publisher object which allows you to
   * publish messages on that topic through a call to publish().  Once
   * all copies of the returned Publisher object are destroyed, the topic
   * will be automatically unadvertised.
   *
   * The second parameter to advertise() is the size of the message queue
   * used for publishing messages.  If messages are published more quickly
   * than we can send them, the number here specifies how many messages to
   * buffer up before throwing some away.
   */
// %Tag(PUBLISHER)%
  ros::Publisher imu_pub = imu.advertise<std_msgs::Int32MultiArray>("imu_debug", 1000);
  ros::Publisher pose_pub = imu.advertise<std_msgs::Int32MultiArray>("pose_corr", 1000);
 //cout<<"Published\n!";
// %EndTag(PUBLISHER)%

// %Tag(LOOP_RATE)%
  ros::Rate loop_rate(1000);
// %EndTag(LOOP_RATE)%

  /**
   * A count of how many messages we have sent. This is used to create
   * a unique string for each message.
   */
// %Tag(ROS_OK)%
  int count = 0;


// %EndTag(ROS_OK)%
 
// main loop for scanning

	double t_dif1=0, t_dif2=0;
	int res=-1;
	char buf[170];
	int cnt=0;
	int frq=0;
        int fail=0;
	double t_dif=0;
	time(&t_start); 
	time(&t1_start);  
		

	int out=0;
	write(fd,"#ad>M==Dj\r",sizeof("#ad>M==Dj\r")/sizeof(char));
  while (ros::ok())
  {
	
			while (1){

				res=read(fd,buf,170);
				if (res!=-1){
					buf[res]=0;
					string gs(buf);
					frq++;
					//cout<<gs<<endl;
					if ((gs.find("#bD")==0)){//&&(gs.find('\r')==0)){
						
						//cout<<"res: "<<res<<" required: "<<gs<<endl;
						unsigned int sum=0,i;
						for (i=0;i<gs.length()-3;i++)
						{
							sum += buf[i];
						}
						//cout<<sum<<":"<<gs.length()<<endl;
						sum %=4096;
						//cout<<char('='+sum/64)<<","<<buf[i]<<int(buf[i])<<endl;
						//cout<<char('='+sum%64)<<","<<buf[i+1]<<int(buf[i+1])<<endl;						
						if (((buf[i]-'='-sum/64)==0)&&((buf[i+1]-'='-sum%64)==0))
						{
							msg.data.clear();
							msg_imu.data.clear();
							out=decode(gs);
							imu_pub.publish(msg);
							pose_pub.publish(msg_imu);
							
							cnt++;
							
						}
						else
						{
							//cout<<"IMU data chech sum not passed"<<endl;
							fail++;
						}
					
						//if (cnt>frq/3){
						//	cout<<"Send! cnt"<<endl;
						//	write(fd,"#ad>M==Dj\r",sizeof("#ad>M==Dj\r")/sizeof(char));
						//}
					}
//					else
//						cout<<"Other data: "<<gs<<endl;
				}
				if (frq>200){
					cout<<"Send! "<<frq<<endl;
					write(fd,"#ad>M==Dj\r",sizeof("#ad>M==Dj\r")/sizeof(char));
					frq=0;
				}
				time(&t_end);
				t_dif = difftime(t_end,t_start);
				if (t_dif>=1){
					cout<<"Updating rate: "<<cnt<<" Packets lost: "<<fail<<endl;
					fail=0;
				//	frq=cnt;
					if (cnt<60){
						cout<<"cnt<60, Send!"<<endl;
						write(fd,"#ad>M==Dj\r",sizeof("#ad>M==Dj\r")/sizeof(char));
					}
					cnt=0;
					time(&t_start);
				}
				if (out==1)
				{
					out=0;	
					break;
				}

			}

			


// %Tag(SPINONCE)%
    ros::spinOnce();
// %EndTag(SPINONCE)%

// %Tag(RATE_SLEEP)%
    loop_rate.sleep();
// %EndTag(RATE_SLEEP)%
    ++count;
  }

//  fclose(fid);
  return 0;
}
// %EndTag(FULLTEXT)%
